#include <vector>
#include <deque>
#include <numeric>
#include <stdexcept>
#include "behaviortree_cpp/action_node.h"
#include "utils/load_data.hpp"

/******************ROS2***************/
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
class DataPreProcessAction : public BT::SyncActionNode {
public:
    DataPreProcessAction(const std::string& name, const BT::NodeConfig& config);
    void new_value_update_mean(float new_value, std::deque<float>& mean_history, size_t window_size = 5000);//默认参数只能在函数声明中指定一次
    float iir_filter(float x);
    float move_averge_filter(float x);

    static BT::PortsList providedPorts() {
        return BT::PortsList ({
                    BT::InputPort<sensorData>("raw_sensor_input", "Description of input"),
                    BT::OutputPort<sensorData>("filtered_sensor_output", "Description of output")
        });
    }
    BT::NodeStatus tick() override;
    private:
    sensorData sensor_data_raw, sensor_data_filtered;
    std::deque<float> mean_tof_history;
    //ROS2
    std::vector<double> msg_buffer;
    std_msgs::msg::Float32MultiArray message;
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr msg_publisher;

    std_msgs::msg::Float32MultiArray raw_data;
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr raw_publisher;
    // 过滤器状态
    std::vector<double> x_history = {0.0, 0.0}; // 保存 x[n-1], x[n-2]
    std::vector<double> y_history = {0.0, 0.0}; // 保存 y[n-1], y[n-2]

    // IIR 系数
    std::vector<double> b = {0.0279, 0.0557, 0.0279};
    std::vector<double> a = {1.0, -1.4755, 0.5869};

    // imu move averge filter 
    float buffer[6] = {0,0,0,0,0,0};
    int index = 0;

    class estMsgPublisher : public rclcpp::Node
    {
      public:
        explicit estMsgPublisher(const std::string& node_name , DataPreProcessAction* pClass);
        ~estMsgPublisher();
      
        private:
        DataPreProcessAction* pTofFusion;
        rclcpp::TimerBase::SharedPtr timer_;

    };
    std::shared_ptr<estMsgPublisher> ros_node_ptr_;
};

